using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using LocalizationTest.Tools;
using System.IO;

namespace LocalizationTest
{
  public class SelfLocalizationResultHistory
  {
    private List<Dictionary<string, WorldState>> history;
    private Dictionary<string, WorldState> currentDictionary;
    private string groundTruthSelfLocator;

    public SelfLocalizationResultHistory()
    {
      Init();
    }
    private void Init()
    {
      groundTruthSelfLocator = "";
      history = new List<Dictionary<string, WorldState>>();
      currentDictionary = new Dictionary<string, WorldState>();
    }
    public void Add(string selfLocator, WorldState worldState)
    {
      currentDictionary.Add(selfLocator, worldState);
      if (selfLocator.StartsWith("ground_truth")) groundTruthSelfLocator = "ground_truth";
    }
    public void FinishFrame()
    {
      history.Add(currentDictionary);
      currentDictionary = new Dictionary<string, WorldState>();
      Draw();
    }
    public void Delete()
    {
      Init();
    }
    public void Draw()
    {
      if(history.Count == 0) return;

      Dictionary<string, double> currentPositionError = new Dictionary<string, double>();
      Dictionary<string, double> currentRotationError = new Dictionary<string, double>();
      Dictionary<string, double> averagePositionError = new Dictionary<string, double>();
      Dictionary<string, double> averageRotationError = new Dictionary<string, double>();
      Dictionary<string, double> counterForAverage = new Dictionary<string, double>();
      Dictionary<string, double> deviationPosition;
      Dictionary<string, double> deviationRotation;
      Dictionary<string, double> counterForDeviation;

      CalculateEachError(currentPositionError, currentRotationError, averagePositionError, averageRotationError, counterForAverage);
      CalculateAverageError(currentPositionError, averagePositionError, averageRotationError, counterForAverage);
      CalculateEachDeviation(currentPositionError, currentRotationError, averagePositionError, averageRotationError, out deviationPosition, out deviationRotation, out counterForDeviation);
      CaclulateAverageDeviation(currentPositionError, deviationPosition, deviationRotation, counterForDeviation);
    }

    private void CalculateEachError(Dictionary<string, double> currentPositionError, Dictionary<string, double> currentRotationError, Dictionary<string, double> averagePositionError, Dictionary<string, double> averageRotationError, Dictionary<string, double> counterForAverage)
    {
      int indexLimit = 0;
      if (DebugKeyTool.IsActive("history:skip first results")) indexLimit = 40;

      int index = 0;
      Dictionary<string, WorldState> lastDictionary = new Dictionary<string, WorldState>();
      foreach (Dictionary<string, WorldState> entry in history)
      {
        if (index > indexLimit)
        {
          foreach (string selfLocator in entry.Keys)
          {
            Color color = GetSelfLocatorColor(selfLocator);
            double shade = Math.Min(
              ((double)history.Count - (double)index) / Math.Min(100.0, (double)history.Count - indexLimit),
              0.75); //(double)history.Count

            color = DrawingTools.GetShadedColor(shade, ref color);
            if (lastDictionary.ContainsKey(selfLocator))
            {
              entry[selfLocator].DrawConnectingLine(
                lastDictionary[selfLocator],
                "world:result:history:" + selfLocator,
                color);
            }
            if (entry.ContainsKey(groundTruthSelfLocator) && groundTruthSelfLocator != "" && entry[groundTruthSelfLocator] != null)
            {
              color = Color.Black;
              color = DrawingTools.GetShadedColor(shade, ref color);
              entry[selfLocator].DrawConnectingLine(
                entry[groundTruthSelfLocator],
                "world:result:comparision to ground truth:" + selfLocator,
                color);
              if (entry[selfLocator] is WorldStatePose2D)
              {
                currentPositionError[selfLocator] =
                  ((entry[selfLocator] as WorldStatePose2D).Pose.translation -
                  (entry[groundTruthSelfLocator] as WorldStatePose2D).Pose.translation).Abs();
                currentRotationError[selfLocator] =
                  MathTools.Normalize(Math.Abs((entry[selfLocator] as WorldStatePose2D).Pose.rotation -
                  (entry[groundTruthSelfLocator] as WorldStatePose2D).Pose.rotation));

                DebugDrawingTool.CreateLine("time_line:comparisionToGroundTruth:" + selfLocator,
                  new Vector2((history.Count - index) * 2, 0),
                  new Vector2((history.Count - index) * 2, currentPositionError[selfLocator]), 2, Color.Red);

                if (!averagePositionError.ContainsKey(selfLocator)) averagePositionError[selfLocator] = 0;
                if (!averageRotationError.ContainsKey(selfLocator)) averageRotationError[selfLocator] = 0;
                averagePositionError[selfLocator] += currentPositionError[selfLocator];
                averageRotationError[selfLocator] += currentRotationError[selfLocator];
                if (!counterForAverage.ContainsKey(selfLocator)) counterForAverage[selfLocator] = 0;
                counterForAverage[selfLocator]++;
              }
            } //if last contains
          } //foreach selflocator
        } // index > ...
        lastDictionary = entry;
        index++;
      }//for each entry in history
    }
    private static void CalculateAverageError(Dictionary<string, double> currentPositionError, Dictionary<string, double> averagePositionError, Dictionary<string, double> averageRotationError, Dictionary<string, double> counterForAverage)
    {
      int z = 0;
      Vector2 offset1 = new Vector2(-400, -300);
      Vector2 offset2 = new Vector2(-150, -300);
      Vector2 offset3 = new Vector2(-50, -300);
      int diff = 12;
      foreach (string selfLocator in currentPositionError.Keys)
      {
        averagePositionError[selfLocator] /= counterForAverage[selfLocator];
        averageRotationError[selfLocator] /= counterForAverage[selfLocator];

        DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
          selfLocator + ": ",
        new Vector2(40, diff * z) + offset1, 10, Color.Black);

        DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
          currentPositionError[selfLocator].ToString("000.0"),
        new Vector2(40, diff * z) + offset2, 10, Color.Black);

        DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
          "avg: " + averagePositionError[selfLocator].ToString("000.0"),
        new Vector2(40, diff * z) + offset3, 10, Color.Black);
        z++;
      }
    }
    private void CalculateEachDeviation(Dictionary<string, double> currentPositionError, Dictionary<string, double> currentRotationError, Dictionary<string, double> averagePositionError, Dictionary<string, double> averageRotationError, out Dictionary<string, double> deviationPosition, out Dictionary<string, double> deviationRotation, out Dictionary<string, double> counterForDeviation)
    {
      int indexLimit = 0;
      if (DebugKeyTool.IsActive("history:skip first results")) indexLimit = 40;
      deviationPosition = new Dictionary<string, double>();
      deviationRotation = new Dictionary<string, double>();
      counterForDeviation = new Dictionary<string, double>();
      int index = 0;

      foreach (Dictionary<string, WorldState> entry in history)
      {
        if (index > indexLimit)
        {
          foreach (string selfLocator in entry.Keys)
          {
            if (entry.ContainsKey(groundTruthSelfLocator) && groundTruthSelfLocator != null && entry[groundTruthSelfLocator] != null)
            {
              if (entry[selfLocator] is WorldStatePose2D)
              {
                currentPositionError[selfLocator] =
                  ((entry[selfLocator] as WorldStatePose2D).Pose.translation -
                  (entry[groundTruthSelfLocator] as WorldStatePose2D).Pose.translation).Abs();
                currentRotationError[selfLocator] =
                  MathTools.Normalize(Math.Abs((entry[selfLocator] as WorldStatePose2D).Pose.rotation -
                  (entry[groundTruthSelfLocator] as WorldStatePose2D).Pose.rotation));

                if (!deviationPosition.ContainsKey(selfLocator)) deviationPosition[selfLocator] = 0;
                if (!deviationRotation.ContainsKey(selfLocator)) deviationRotation[selfLocator] = 0;
                deviationPosition[selfLocator] += Math.Pow(currentPositionError[selfLocator] - averagePositionError[selfLocator], 2);
                deviationRotation[selfLocator] += Math.Pow(currentRotationError[selfLocator] - averageRotationError[selfLocator], 2);
                if (!counterForDeviation.ContainsKey(selfLocator)) counterForDeviation[selfLocator] = 0;
                counterForDeviation[selfLocator]++;
              }
            } //if last contains
          } //foreach selflocator
        } // index > ...
        index++;
      }//for each entry in history
    }
    private static void CaclulateAverageDeviation(Dictionary<string, double> currentPositionError, Dictionary<string, double> deviationPosition, Dictionary<string, double> deviationRotation, Dictionary<string, double> counterForDeviation)
    {
      int z = 0;
      Vector2 offset3 = new Vector2(+50, -300);
      int diff = 12;
      foreach (string selfLocator in currentPositionError.Keys)
      {
        deviationPosition[selfLocator] /= counterForDeviation[selfLocator];
        deviationRotation[selfLocator] /= counterForDeviation[selfLocator];

        deviationPosition[selfLocator] = Math.Sqrt(deviationPosition[selfLocator]);
        deviationRotation[selfLocator] = Math.Sqrt(deviationRotation[selfLocator]);

        /*
                DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
                  selfLocator + ": ",
                new Vector2(40, diff * z) + offset1, 10, Color.Black);

                DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
                  currentPositionError[selfLocator].ToString("000.0"),
                new Vector2(40, diff * z) + offset2, 10, Color.Black);
                */
        DebugDrawingTool.CreateText("world:result:deviation:" + selfLocator,
          "+-: " + deviationPosition[selfLocator].ToString("000.0"),
        new Vector2(40, diff * z) + offset3, 10, Color.Black);
        z++;
      }
    }

    public static Color GetSelfLocatorColor(string selflocator)
    {
      if (selflocator == "ground_truth") return Color.Green;
      else if (selflocator == "odometry") return Color.Red;
      else if (selflocator == "BayesSelfLocator") return Color.Orange;
      else if (selflocator == "MBLSimpleGradientSelfLocator default") return Color.Brown;
      else if (selflocator == "MBLSimpleGradientSelfLocator last20") return Color.DarkOrange;
      else if (selflocator == "MBLSimpleGradientSelfLocator last5OfEach") return Color.DarkOrchid;
      else if (selflocator == "MBLSimpleGridRefinement") return Color.DeepSkyBlue;
      else if (selflocator == "MBLSimpleGridRefinement2 default") return Color.Olive;
      return Color.DeepSkyBlue;
    }

  }
}
